from numpy import *
from scipy import linalg
class Camera(object):
    """描述针孔相机的类"""
    from scipy import linalg

    def __init__(self,P):
        """初始化相机模型P=K[R|t]"""
        self.P=P
        self.K=None #校正矩阵
        self.R=None #旋转矩阵
        self.t=None #平移
        self.c=None #相机中心

    def project(self,X):
        """将空间中点X数组（4*n)进行投影，并normalize coordinates """
        x=dot(self.P,X)
        for i in range(3):
            x[i]/=x[2];
        return x

    def rotation_matrix(a):
        """创建一个以a向量为轴的3D旋转矩阵，旋转的角度为a的模值
        可以参见《Robotics Vision&control》一书的2.2.1.5节
        """
        R =eye(4)
        R[:3,:3] = linalg.expm([[0,-a[2],a[1]],[a[2],0,-a[0]],[-a[1],a[0],0]])
        return R

    def factor(self):
        """将相机矩阵分解为K,R,t, P=K[R|t].
        参见《multiple view geometry in computer vision》的6.2.4节
        """
        K,R=linalg.rq(self.P[:,:3]) #RQ分解
        #确保K的对角元为正
        T=diag(sign(diag(K)))
        if linalg.det(T)<0:
            T[1,1]*=-1
        self.K =dot(K,T)
        self.R =dot(T,R)
        self.t=dot(linalg.inv(self.K),P[:,3])
        return self.K, self.R, self.t

    def center(self):
        """计算并返回相机的中心
        参见《multiple view geometry in computer vision》的6.2.4节
        """
        if self.c is not None:
            return self.c
        else:
            self.factor()
            self.c=-dot(self.R.T, self.t)
            return self.c


###########test####################
'''
#载入3D点数据
points=loadtxt('house.p3d').T   #得到3*n的数组
points=vstack((points, ones(points.shape[1])) )

#创建camera
P=hstack((eye(3),array([[0],[0],[-10] ])))
cam=Camera(P)
x=cam.project(points)

#显示投影点
#figure()
#plot(x[0],x[1],'k.')
#show()

#创建变换
r=0.05*random.rand(3)   #生成3个0~1之间的随机数
rot=Camera.rotation_matrix(r)   #产生一个绕r向量的旋转
'''


"""
#旋转相机及投影
figure()
for t in range(20): #连续的旋转20次
    cam.P=dot(cam.P,rot);
    x=cam.project(points)
    plot(x[0],x[1],'k.')
show()
"""
